/*
 * @Description: 
 * @Author: Sang Hao
 * @Date: 2021-11-09 19:12:06
 * @LastEditTime: 2021-11-12 21:22:24
 * @LastEditors: Sang Hao
 */

#ifndef LIDAR_SLAM_MAPPING_SEG_MAP_SEG_TRUTH_HPP_
#define LIDAR_SLAM_MAPPING_SEG_MAP_SEG_TRUTH_HPP_

#include <vector>
#include <ros/ros.h>
#include <yaml-cpp/yaml.h>

#include "lidar_slam/models/segmentation/cloud_segment_interface.hpp"
#include "lidar_slam/sensor_data/cloud_data.hpp"
#include "lidar_slam/sensor_data/cloud_rgb_data.hpp"
#include "lidar_slam/publisher/cloud_publisher.hpp"
#include "lidar_slam/models/extract/plane_ransac_extract.hpp"

namespace lidar_slam {
class RansacClusterSegment : public CloudSegmentInterface {
private:
	// std::shared_ptr<CloudPublisher> cloud_pub_ptr_;
	std::shared_ptr<PlaneRansacExtract> horizontal_plane_extract_;
	std::shared_ptr<PlaneRansacExtract> vertical_plane_extract_;
	
	/* 因为需要传参数 */
	pcl::ModelCoefficients::Ptr model_coeff_ptr_; 
	pcl::PointIndices::Ptr point_indices_ptr_;

	std::vector<pcl::ModelCoefficients::Ptr> vec_model_coeff_ptr_;	
	std::vector<pcl::PointIndices::Ptr> vec_point_indices_ptr_;
	
	/* 这个需要传参数 */
	std::vector<pcl::PointIndices> vec_cluster_indices_;

	/* 作为结果输出，这样函数就好写了，各个函数提取到的索引都加进来 */
	CloudRGBData::CLOUD_PTR seged_cloud_ptr_;

	/* params */
	int min_cluster_size_;
	float cluster_tolerance_;

public:
	RansacClusterSegment(const YAML::Node& config_node);

	// bool HorizontalPlaneFilter(const CloudData::CLOUD_PTR& input_cloud_ptr, 
							//    CloudData::CLOUD_PTR& filtered_cloud_ptr);

	// bool VerticalPlaneFilter(const CloudData::CLOUD_PTR& input_cloud_ptr, 
							//  CloudData::CLOUD_PTR& filtered_cloud_ptr);
	
	bool EuclideanClusterExtract(const CloudData::CLOUD_PTR& input_cloud_ptr);

	bool Segmentation(const CloudData::CLOUD_PTR& input_cloud_ptr, 
					CloudRGBData::CLOUD_PTR& seged_cloud_ptr) override;

};
}
#endif